#include "canopen.h"

canopen::canopen()
{

}

struct can_frame canopen_sdo_write(uint8_t node_id, uint16_t index, uint8_t subindex, uint32_t data,int type)
{
    struct can_frame frame = {0};
    uint8_t type_cs[4]={0x2f,0x2b,0x27,0x23};
   frame.can_id = CANOPEN_SDO_WRITE_REQUEST(node_id); // COB-ID
   frame.can_dlc = 8;

   frame.data[0] = type_cs[type]; // SDO写命令
   frame.data[1] = index & 0xFF;      // Index低字节
   frame.data[2] = (index >> 8) & 0xFF; // Index高字节
   frame.data[3] = subindex;          // Subindex
   switch (type) {
   case WRITE_BYTE_1:
       frame.data[4]  = data&0xff;
       break;
   case WRITE_BYTE_2:
       frame.data[4]  = data&0xff;
       frame.data[5]  = (data>>8)&0xff;
       break;
   case WRITE_BYTE_3:
       frame.data[4]  = data&0xff;
       frame.data[5]  = (data>>8)&0x00000ff;
       frame.data[6]  = (data>>16)&0x00000ff;
       break;
   case WRITE_BYTE_4:
       frame.data[4]  = data&0xff;
       frame.data[5]  = (data>>8)&0x00000ff;
       frame.data[6]  = (data>>16)&0x00000ff;
       frame.data[7]  = (data>>24)&0x00000ff;
       break;
   default:
       break;
   }
   return  frame;
}

struct can_frame canopen_sdo_read(uint8_t node_id, uint16_t index, uint8_t subIndex)
{
    struct can_frame frame;
    // 构造 CANopen SDO 请求帧
    frame.can_id = CANOPEN_SDO_WRITE_REQUEST(node_id);
    frame.can_dlc = 8;
    frame.data[0] = CANOPEN_SDO_READ_CMD;  // 读命令
    frame.data[1] = index & 0xFF;         // Index LSB
    frame.data[2] = (index >> 8) & 0xFF;  // Index MSB
    frame.data[3] = subIndex;             // 子索引
    frame.data[4] = 0;
    frame.data[5] = 0;
    frame.data[6] = 0;
    frame.data[7] = 0;
    return frame;
}
